/*
 * init.c
 *
 *  Created on: 2024年7月4日
 *      Author: Kenobi
 */

#include "zf_common_headfile.h"
#include "init.h"

Target_Speed_Struct Target_Speed;
int16_t timer_1000ms=0;

void mytft_init()
{
    tft180_set_dir(TFT180_PORTAIT);
    tft180_init();
}

void motor_init()
{
    pwm_init(PWM_CH5, 5000, 10000);                                                // 初始化 PWM 通道 频率 17KHz 初始占空比 100%
    pwm_init(PWM_CH6, 5000, 10000);                                                // 初始化 PWM 通道 频率 17KHz 初始占空比 100%
    pwm_init(PWM_CH7, 5000, 10000);                                                // 初始化 PWM 通道 频率 17KHz 初始占空比 100%
    pwm_init(PWM_CH8, 5000, 10000);                                                // 初始化 PWM 通道 频率 17KHz 初始占空比 100%
}

void motoir(uint16_t duty1,uint16_t duty2,uint16_t duty3,uint16_t duty4)
{
    pwm_set_duty(PWM_CH5, 10000-duty1);
    pwm_set_duty(PWM_CH6, 10000-duty2);
    pwm_set_duty(PWM_CH7, 10000-duty3);
    pwm_set_duty(PWM_CH8, 10000-duty4);
}

void BLDC_init()
{
    pwm_init(PWM_CH1, 50, 0);
    pwm_init(PWM_CH2, 50, 0);
    pwm_init(PWM_CH3, 50, 0);
    pwm_init(PWM_CH4, 50, 0);
    pwm_init(PWM_CH5, 50, 0);
    pwm_init(PWM_CH6, 50, 0);
}

void BLDE_Side(uint16 duty1,uint16 duty2)
{
    duty1 = func_limit_ab(duty1,500,900);
    duty2 = func_limit_ab(duty2,500,900);

    pwm_set_duty(PWM_CH5, duty1);
    pwm_set_duty(PWM_CH6, duty2);
}

void BLDC(uint16 duty1,uint16 duty2,uint16 duty3,uint16 duty4)
{
    //无刷
    // 计算无刷电调转速   （1ms - 2ms）/20ms * 10000（10000是PWM的满占空比时候的值）
    // 无刷电调转速 0%   为 500
    // 无刷电调转速 20%  为 600
    // 无刷电调转速 40%  为 700
    // 无刷电调转速 60%  为 800
    // 无刷电调转速 80%  为 900
    // 无刷电调转速 100% 为 1000
    duty1 = func_limit_ab(duty1,500,900);
    duty2 = func_limit_ab(duty2,500,900);
    duty3 = func_limit_ab(duty3,500,900);
    duty4 = func_limit_ab(duty4,500,900);

    pwm_set_duty(PWM_CH1, duty1);
    pwm_set_duty(PWM_CH2, duty2);
    pwm_set_duty(PWM_CH3, duty3);
    pwm_set_duty(PWM_CH4, duty4);
}

void encoder_init()
{
    encoder_quad_init(ENCODER_QUADDEC, ENCODER_QUADDEC_A, ENCODER_QUADDEC_B); // 初始化编码器模块与引脚 正交解码编码器模式
    pit_ms_init(encoder_PIT_CH, 10);                                         // 初始化 PIT 为周期中断 5ms 周期
    interrupt_set_priority(encoder_PIT_PRIORITY, (0 << 5) | 0);               // 设置 PIT 对周期中断的中断抢占优先级0,子优先级1
}

void kamera_init()
{
    while(mt9v03x_init())
    {
        printf("mt9v03x_init error!!!");
    }
//    system_delay_ms(1000);
}

void imu660_init()
{
    imu660ra_init();
    memset(&Gyroscope, 0, sizeof Gyroscope);                //初始化陀螺仪
    Gyroscope.FJ_Angle_Max = 360;
    Gyroscope.FJ_Angle_Min = -360;

    pit_ms_init(imu660_PIT_CH, 10);
    interrupt_set_priority(imu660_PIT_PRIORITY, (0 << 5) | 1); // 抢占优先级设置为1,次优先级设置为1
    Zero_Point_Detect();                                    //读取零飘
}

void my_timer_init()
{
//    pit_ms_init(TIM7_PIT, 10);                        // 用于处理pid计算的定时器
//    interrupt_set_priority(TIM7_IRQn, (0 << 5) | 3);            // 抢占优先级设置为0,次优先级设置为0

    pit_ms_init(TIM3_PIT, 10);                        // 按键
    interrupt_set_priority(TIM3_IRQn, (0 << 5) | 2); // 抢占优先级设置为0,次优先级设置为2

}

void my_exit_init()
{
    exti_init(A8,EXTI_TRIGGER_FALLING);
    exti_init(C7,EXTI_TRIGGER_FALLING);
    exti_init(C6,EXTI_TRIGGER_FALLING);
    exti_init(D14,EXTI_TRIGGER_FALLING);
}

void var_init() //变量初始化
{
    memset(&Island, 0, sizeof Island);
    memset(&Image_Change, 0, sizeof Image_Change);
    memset(&battery, 0, sizeof battery);
    memset(&Lost, 0, sizeof Lost);
    memset(&LonWCol, 0, sizeof LonWCol);
    memset(&Line, 0, sizeof Line);
    memset(&Cross, 0, sizeof Cross);
    memset(&Gyroscope, 0, sizeof Gyroscope);
    memset(&Barrier, 0, sizeof Barrier);
    memset(&Target_Speed, 0, sizeof Target_Speed);
    memset(&Set_Pid, 0, sizeof Set_Pid);
    memset(&Zebra_Stripes, 0, sizeof Zebra_Stripes);

    //pid系数
//    Set_Pid.C_kp=0.4;Set_Pid.C_kd=150;
//    Set_Pid.S_kp=4.2;Set_Pid.S_kd=60;

    Set_Pid.C_kp=0.2;Set_Pid.C_kd=160;
    Set_Pid.S_kp=5;Set_Pid.S_kd=100;

    Set_Pid.Cc_kp=25;Set_Pid.Cc_kd=50;

    Set_Pid.Sp_kp=3.3;Set_Pid.Sp_ki=0.0;

    //基准速度
    Target_Speed.Left_Baseline_Speed=710;
    Target_Speed.Right_Baseline_Speed=710;

    //下无刷
    Target_Speed.Down_Target_Speed=650;

    //直道速度
    Target_Speed.Straight_Target_Speed=200;
//    Write_Flash();
}

void var_usr()
{
    Target_Speed.Down_Target_Speed_Out = Target_Speed.Down_Target_Speed;
    Target_Speed.Target_Speed_Out = Target_Speed.Straight_Target_Speed;
    //速度环
    Speed_Pid.kp=Set_Pid.Sp_kp;
    Speed_Pid.ki=Set_Pid.Sp_ki;

    //转向环
    Speed_Pd.kp=Set_Pid.S_kp;
    Speed_Pd.kd=Set_Pid.S_kd;
    //转向环
    Cascade_Pd.kp=Set_Pid.C_kp;
    Cascade_Pd.kd=Set_Pid.C_kd;
    //转向环
    Side_Cascade_Pd.kp=Set_Pid.Cc_kp;
    Side_Cascade_Pd.kd=Set_Pid.Cc_kd;

}
